#include <stdio.h>
#include <unistd.h>
#include "datamanger.h"
#include "communicate.h"
#include "odom.h"
DATAMANGER_STRUCT robotinfo[MAXINDEX];  //全局变量结构体数组
ACTION_STRUCT event;                    //处理 前进距离、转动角度等 事件  的结构体

/**
 * 初始化数据结构和数据
 * 初始化事件管理结构体
 * */
void robotinfo_init(void)
{
    int i ;
    /**********初始化数据结构和数据***********/
    for(i = 0; i < MAXINDEX; i++)
    {
        pthread_mutex_init(&(robotinfo[i].data_lock), NULL);    //初始化所有互斥锁
    }   

    //全局变量初始值
    robotinfo[LINE_WARNING].data = 0;       //巡线关闭
    robotinfo[AROUND_SPEED].data = 0;       //巡检速度默认为0
    robotinfo[GYRO].data = 0;               //陀螺仪数据初始化
    robotinfo[WORK_MODE].data = 0;          //默认机器人停止工作 

    /**********初始化事件管理结构体***********/
    pthread_mutex_init(&event.action_mutex, NULL);
    pthread_cond_init(&event.event_cond, NULL);
}


/**
 * 全局变量读
 * 
 * */
int32_t robotinfo_read(DATAMANGER_INDEX index)
{
    int32_t retval;
    if(index < MAXINDEX)
    {
        pthread_mutex_lock(&(robotinfo[index].data_lock));
        
        retval = robotinfo[index].data;

        pthread_mutex_unlock(&(robotinfo[index].data_lock));
    }
    else{
        printf("robot info read index error!!!");
    }

    return retval;
}

/**
 * 全局变量写
 * 变量名参考：DATAMANGER_INDEX
 * */
void robotinfo_write(DATAMANGER_INDEX index , int32_t temp)
{
    if(index < MAXINDEX)
    {
        pthread_mutex_lock(&(robotinfo[index].data_lock));
        robotinfo[index].data = temp;
        pthread_mutex_unlock(&(robotinfo[index].data_lock));
    }
    else{
        printf("robot info write index error!!!");
    }

}

/**
 * 触发某个事件
 * 事件类型参考enum 
 * */
void trigger_event(EVENT_INDEX index, int32_t target_val, int32_t param, int32_t follow_act)
{
    pthread_mutex_lock(&event.action_mutex);

    event.action = index;
    event.target_value = target_val;
    event.follow_action = follow_act;

    pthread_mutex_unlock(&event.action_mutex);
    pthread_cond_signal(&event.event_cond);
}


/**
 * 获取当前的事件，如果没有时间函数将 "阻塞"
 * 返回待处理的事件
 * */
EVENT_INDEX get_current_event(void)
{
    EVENT_INDEX ret_event = 0;
    pthread_mutex_lock(&event.action_mutex);

    while(event.action == 0){ 
        pthread_cond_wait(&event.event_cond, &event.action_mutex);
    }
    ret_event = event.action;
    event.action = 0;       //清除动作记录
    pthread_mutex_unlock(&event.action_mutex);

    return ret_event;

}

int32_t get_event_targetval(void) 
{
    int32_t ret_val;
    pthread_mutex_lock(&event.action_mutex);
    ret_val = event.target_value;
    pthread_mutex_unlock(&event.action_mutex);

    return ret_val;
}
int32_t get_action_param(void)
{
    int32_t ret_param;
    pthread_mutex_lock(&event.action_mutex);
    ret_param = event.action_param;
    pthread_mutex_unlock(&event.action_mutex);

    return ret_param;
}

int32_t get_event_followact(void)
{
    int32_t ret_follow;
    pthread_mutex_lock(&event.action_mutex);
    ret_follow = event.follow_action;
    pthread_mutex_unlock(&event.action_mutex);

    return ret_follow;
}

void clear_event(void)
{
    pthread_mutex_lock(&event.action_mutex);
    event.action = EVENT_NULL;
    event.follow_action = 0;
    event.target_value = 0;
    event.action_param = 0;
    pthread_mutex_unlock(&event.action_mutex);

}




void updata_robot_info(DATAMANGER_INDEX index)
{
    int32_t temp_data;

    temp_data = robotinfo_read(index);
    eth_cmd_package(0, THE_INFO,index,temp_data);
}

void updata_robot_odom(void)
{
    int32_t position_temp32;
    ROBOT_POSITION cur_position;

    read_odom(&cur_position);
    position_temp32 = (int32_t) ((uint8_t) (cur_position.dual & 0xff)) | ((uint16_t) (cur_position.dist_to_radpoint & 0xffff) );
   // eth_cmd_package(0 , THE_INFO,  position_temp32);   
    
}